
#include <stddef.h>
#include <string.h>
#include <assert.h>

#include "MessageBus.h"
#include "MessagePool.h"
#include "MessageMemory.h"
#include "internal/MessageQueue.h"

#ifndef NDEBUG
#define MsgBus_Assert(test)	assert(test)
#else
#define MsgBus_Assert(test)	(void)(0)
#endif

#if MsgBusAddr_MAX <= 8
	typedef uint8_t Subscribe_T;
#elif MsgBusAddr_MAX <= 16
	typedef uint16_t Subscribe_T;
#elif MsgBusAddr_MAX <= 32
	typedef uint32_t Subscribe_T;
#else
	#error "MsgBusAddr_MAX > 32"
#endif

#define MsgBus_PublishAddr		0xFF

static MsgHandler_T msgHandlerTable[MsgBusAddr_MAX];
static Subscribe_T msgSubscribeList[MsgType_MAX];

void MsgBus_Register(MsgBusAddr_T addr, MsgHandler_T handler){
	msgHandlerTable[addr] = handler;
}

void MsgBus_Unregister(MsgBusAddr_T addr){
	msgHandlerTable[addr] = NULL;
}

void MsgBus_Init(void){
	memset(msgHandlerTable, 0, sizeof(msgHandlerTable));
	memset(msgSubscribeList, 0, sizeof(msgSubscribeList));
	MsgPool_Init();
	MsgQueue_Init();
	MsgMem_Init();
}

void MsgBus_Deinit(void){
	MsgQueue_Deinit();
}

int MsgBus_Send(Message_T* msg){
	MsgBus_Assert(msg);
	MsgBus_Assert(msg->dest < MsgBusAddr_MAX || msg->dest == MsgBus_PublishAddr);

	if(msg->dest < MsgBusAddr_MAX || msg->dest == MsgBus_PublishAddr){
		Message_T* m = Message_Clone(msg);
		if(!m){
			return -2;
		}
		return MsgQueue_Post(m);
	}
	return -1;
}

int MsgBus_SendData(MsgBusAddr_T dest, MsgBusAddr_T src, MsgType_T type, MsgAttr_T attr, void *data){
	MsgBus_Assert(attr < 128);
	Message_T msg = {dest, src, type, attr | 0x80, .data = data};
	return MsgBus_Send(&msg);
}

int MsgBus_DynamicSend(MsgBusAddr_T dest, MsgBusAddr_T src, MsgType_T type, MsgAttr_T attr, void const *data, uint32_t size){
	MsgBus_Assert(attr < 128);
	void* buf = MsgMem_Alloc(size);
	if(!buf){
		return -3;
	}
	memcpy(buf, data, size);
	Message_T msg = {dest, src, type, attr | 0x80, .data = buf};
	return MsgBus_Send(&msg);
}

int MsgBus_SendVal(MsgBusAddr_T dest, MsgBusAddr_T src, MsgType_T type, MsgAttr_T attr, uint32_t val){
	MsgBus_Assert(attr < 128);
	Message_T msg = {dest, src, type, attr, .val = val};
	return MsgBus_Send(&msg);
}


int MsgBus_Publish(Message_T* msg){
	MsgBus_Assert(msg);
	msg->dest = MsgBus_PublishAddr;
	return MsgBus_Send(msg);
}

int MsgBus_PublishData(MsgBusAddr_T src, MsgType_T type, MsgAttr_T attr, void *data){
	MsgBus_Assert(attr < 128);
	Message_T msg = {MsgBus_PublishAddr, src, type, attr | 0x80, .data = data};
	return MsgBus_Send(&msg);
}

int MsgBus_DynamicPublish(MsgBusAddr_T src, MsgType_T type, MsgAttr_T attr, void const *data, uint32_t size){
	return MsgBus_DynamicSend(MsgBus_PublishAddr, src, type, attr, data, size);
}

int MsgBus_PublishVal(MsgBusAddr_T src, MsgType_T type, MsgAttr_T attr, uint32_t val){
	MsgBus_Assert(attr < 128);
	Message_T msg = {MsgBus_PublishAddr, src, type, attr, .val = val};
	return MsgBus_Send(&msg);
}

void MsgBus_Subscribe(MsgType_T type, MsgBusAddr_T addr){
	if(addr < MsgBusAddr_MAX && type < MsgType_MAX){
		msgSubscribeList[type] |= 1u << addr;
	}
}

void MsgBus_Unsubscribe(MsgType_T type, MsgBusAddr_T addr){
	if(addr < MsgBusAddr_MAX && type < MsgType_MAX){
		msgSubscribeList[type] &= ~(1u << addr);
	}
}

static uint_fast8_t MsgBus_LOG2(Subscribe_T x) {
    static uint8_t const log2LUT[16] = {
        (uint8_t)0, (uint8_t)1, (uint8_t)2, (uint8_t)2,
        (uint8_t)3, (uint8_t)3, (uint8_t)3, (uint8_t)3,
        (uint8_t)4, (uint8_t)4, (uint8_t)4, (uint8_t)4,
        (uint8_t)4, (uint8_t)4, (uint8_t)4, (uint8_t)4
    };
    uint_fast8_t n = (uint_fast8_t)0;
    Subscribe_T    t;

#if (MsgBusAddr_MAX > 16)
    t = (Subscribe_T)(x >> 16);
    if (t != (Subscribe_T)0) {
        n += (uint_fast8_t)16;
        x = t;
    }
#endif
#if (MsgBusAddr_MAX > 8)
    t = (x >> 8);
    if (t != (Subscribe_T)0) {
        n += (uint_fast8_t)8;
        x = t;
    }
#endif
    t = (x >> 4);
    if (t != (Subscribe_T)0) {
        n += (uint_fast8_t)4;
        x = t;
    }
    return n + (uint_fast8_t)log2LUT[x];
}

static void MsgBus_Gc(Message_T *msg){
	if(msg->attr & 0x80){
		MsgMem_Free(msg->data);
	}
	Message_Del(msg);
}


void MsgBus_Dispatch(int ms){
	Message_T* msg;
	
	while(!MsgQueue_Get(&msg, ms)){
		MsgHandler_T h;
		
		if(msg->dest == MsgBus_PublishAddr){
			int i;
			Subscribe_T subs = msgSubscribeList[msg->type];
		
			while(subs != (Subscribe_T)0){
				i = MsgBus_LOG2(subs) - 1;
				h = msgHandlerTable[i];
				if(h){
					h(msg);
				}
				subs &= ~(1u << i);
			}
		}
		else{
			h = msgHandlerTable[msg->dest];
			if(h){
				h(msg);
			}
		}
		MsgBus_Gc(msg);
	}
}


